perm filename MAKETF.SAI[AL,HE]1 blob
sn#290130 filedate 1977-06-28 generic text, type C, neo UTF8
COMMENT ⊗ VALID 00006 PAGES
C REC PAGE DESCRIPTION
C00001 00001
C00002 00002 IFCR ¬DECLARATION(EXTENDED_COMPILATION)
C00005 00003 ! Declarations
C00013 00004 ! GETNUMS
C00015 00005 ! MAKETFILE
C00021 00006 ! Intermediate points
C00030 ENDMK
C⊗;
IFCR ¬DECLARATION(EXTENDED_COMPILATION)
THENC
DEFINE EXTENDED_COMPILATION = "TRUE";
ENTRY;
BEGIN "ttest"
COMMENT: Source file requirements;
REQUIRE "ABBREV.SAI[AL,HE]" SOURCE_FILE;
REQUIRE "MACROS.SAI[AL,HE]" SOURCE_FILE;
REQUIRE "RECAUX.HDR[AL,HE]" SOURCE_FILE;
REQUIRE "ARITH.HDR[AL,HE]" SOURCE_FILE;
REQUIRE "HALREC.SAI[AL,HE]" SOURCE_FILE ;
REQUIRE "EMITER.HDR[AL,HE]" SOURCE_FILE;
REQUIRE "INTDEF.SAI[AL,HE]" SOURCE_FILE;
DEFINE $$PRGID "[]" = ["ttest"];
ENDC;
REQUIRE "EMITE2.REL[AL,HE]" LOAD_MODULE;
REQUIRE "TCALC.HDR[AL,HE]" SOURCE_FILE;
! This is a revision of TTEST1 for the purpose of displaying the
final trajectories on the displays. It takes the several points of
the trajectory as interactive input and produces a traj file
TTEST.DAT which can be examined by TRJDPY. Currently
uses standard orientation;
! Declarations;
INTEGER COUNT, BRCHAR, EOF, FLAG, TR, J;
ITEMVAR WORLD;
STRING STR, CHAR;
RANY PTR, CEL;
RPTR(ROTN) ROT;
RPTR(VIA) VIAPTR;
RPTR(DEPROACH) DEPPTR;
STRING INPSTR;
RPTR(MOVE$) MOV;
PRELOAD_WITH 1., 0., 0., 1., 0., 0., 0., 0., -1.;
REAL OWN ARRAY ROTARRAY [1:3,1:3]; ! Orientation array;
REAL ARRAY SCRATCH [1:6]; ! For holding joint angles, etc;
REAL ARRAY SCRA1 [1:6]; ! For holding joint angles, etc;
INTERNAL INTEGER PROCEDURE GENLAB; RETURN(0);
EXTERNAL BOOLEAN CAUTIOUS; ! If TRUE then motions are slower;
REQUIRE 10 NEW_ITEMS;
! NEW_VAR & GET_VALUE are here since HALREC needs them for an initialization
procedure. They come from EXPRS.;
INTERNAL RPTR(VARIABLE) PROCEDURE NEW_VAR(RANY ITEMVAR IV;
INTEGER DT;RPTR(BLOCK) BID);
BEGIN
RPTR(VARIABLE) VAR;
VAR←NEW_RECORD(VARIABLE);
VARIABLE:NAME[VAR]←IV;
∂(IV)←VAR;
VARIABLE:DATATYPE[VAR]←DT;
VARIABLE:BLK[VAR]←BID;
RETURN(VAR);
END;
INTERNAL RECURSIVE RPTR(VALU$) PROCEDURE GETVALUE(RPTR(VARIABLE) VAR;
ITEMVAR WLD);
BEGIN
PRINT('12&'15&"GETVALUE gets called."&'12&'15);
RETURN(NULL_RECORD);
END;
! GETNUMS;
BOOLEAN PROCEDURE GETNUMS
(REAL ARRAY NUMS; REFERENCE STRING INPSTR; INTEGER HOWMANY);
BEGIN "getnums" ! Ask the user to supply a set of HOWMANY numbers, and
return them in NUMS[1:3];
INTEGER I, WIDTH, DIGITS;
REAL TEMP;
STRING SUGGEST;
SUGGEST ← "";
OUTSTR(CRLF & "Please give me " & CVS(HOWMANY) & " real numbers: ");
GETFORMAT(WIDTH,DIGITS);
SETFORMAT(3,0);
FOR I ← 1 STEP 1 UNTIL HOWMANY DO
SUGGEST ← SUGGEST & CVF(NUMS[I]);
SETFORMAT(WIDTH,DIGITS);
LODED(SUGGEST & CR);
SUGGEST ← INPSTR ← INCHWL;
FOR I ← 1 STEP 1 UNTIL HOWMANY DO
BEGIN "pick" ! Pick off one number;
TEMP ← REALSCAN(SUGGEST,BRCHAR);
IF BRCHAR = -1
THEN RETURN(FALSE)
ELSE NUMS[I] ← TEMP;
END "pick";
RETURN(TRUE);
END "getnums";
! MAKETFILE;
INTERNAL PROCEDURE INITMTF;
BEGIN
! For the time being, use standard orientation;
ROT ← NEW_RECORD(ROTN);
MEMLOC(ROTN:RMX[ROT],INTEGER) ← MEMLOC(ROTARRAY,INTEGER);
END;
INTERNAL PROCEDURE MAKETFILE (REFERENCE STRING MES);
BEGIN "maketfile" ! Main driver loop, repeated over and over;
INTEGER FRILLS;
INTEGER ARM; ! 0=YELLOW, 1=BLUE, 2=YFINGERS, 3=BFINGERS;
INITOUT("TTEST.DAT",NULL); ! Sets up output file;
FRILLS ← CAUTIOUS ← FALSE;
OUTSTR(CRLF & "Frills or Plain? ");
LODED("P" & CRLF);
STR ← INCHWL;
CHAR ← LOP(STR);
IF CHAR="F" ∨ CHAR="f"
THEN BEGIN
FRILLS ← TRUE;
OUTSTR(CRLF & "Cautious? ");
LODED("N" & CRLF);
STR ← INCHWL;
CHAR ← LOP(STR);
IF CHAR="Y" ∨ CHAR="y"
THEN CAUTIOUS ← TRUE
END;
OUTSTR("0=Yarm, 1=Barm, 2=Yhand, 3=Bhand:");
LODED("0" & CRLF);
STR ← INCHWL;
ARM ← INTSCAN(STR,BRCHAR);
! First, make a move record;
MOV ← NEW_RECORD(MOVE$);
MOVE$:WHAT[MOV] ← MOVE$:CF[MOV] ←
CASE ARM OF (YARM, BARM, YHAND, BHAND); ! Defined in HALREC;
MOVE$:DEST[MOV] ← NEW_RECORD(VARIABLE);
MOVE$:DEXP[MOV] ← NEW_RECORD(DEXPR);
MES ← "";
! Initial point;
OUTSTR(CRLF & "Initial place, please. ");
IF ARM ≤ 1
THEN BEGIN ! An arm;
SCRATCH[1] ← 44.; ! Initial suggestion;
SCRATCH[2] ← 57.; ! Initial suggestion;
SCRATCH[3] ← 11.; ! Initial suggestion;
GETNUMS(SCRATCH,INPSTR,3);
MES ← MES & " |" & INPSTR;
PTR ← NEW_FRAME(NEW_TRANS(ROT,NEW_V3ECT
(
SCRATCH[1], ! X coordinate;
SCRATCH[2], ! Y coordinate;
SCRATCH[3] ! Z coordinate;
)));
END
ELSE BEGIN ! A hand;
SCRATCH[1] ← 1; ! Initial suggestion;
GETNUMS(SCRATCH,INPSTR,1);
MES ← MES & " |" & INPSTR;
PTR ← NEW_SVAL(SCRATCH[1]);
END;
VARIABLE:VAL[MOVE$:WHAT[MOV]] ← PTR;
! Clauses;
CEL ← MOVE$:CLAUSES[MOV] ← NEW_RECORD(CELL);
IF FRILLS AND (ARM ≤ 1)
THEN BEGIN "departure"
OUTSTR(CRLF & "Departure point; blank means none. ");
IF GETNUMS(SCRATCH,INPSTR,3)
THEN BEGIN ! A departure point has been requested;
MES ← MES & " |D:" & INPSTR;
PTR ← NEW_FRAME(NEW_TRANS(ROT,NEW_V3ECT(
SCRATCH[1], ! X coordinate;
SCRATCH[2], ! Y coordinate;
SCRATCH[3] ! Z coordinate;
)));
DEPPTR ← CELL:CAR[CEL] ← NEW_RECORD(DEPROACH);
DEPROACH:FRAME[DEPPTR] ← NEW_RECORD(VARIABLE);
VARIABLE:VAL[DEPROACH:FRAME[DEPPTR]] ← PTR;
DEPROACH:DEPARTURE[DEPPTR] ← TRUE;
CEL ← CELL:CDR[CEL] ← NEW_RECORD(CELL);
END;
END "departure";
! Intermediate points;
WHILE TRUE DO
BEGIN "stuff" ! Each iteration handles one via point. We use
standard orientation ROT;
BOOLEAN VIAS;
VIAS ← FALSE;
OUTSTR(CRLF & "Via point, please; blank means no more. ");
IF ARM ≤ 1
THEN BEGIN ! An arm;
IF GETNUMS(SCRATCH,INPSTR,3)
THEN BEGIN "avia" ! A via point has been requested;
VIAS ← TRUE;
MES ← MES & " |" & INPSTR;
PTR ← NEW_FRAME(NEW_TRANS(ROT,NEW_V3ECT(
SCRATCH[1], ! X coordinate;
SCRATCH[2], ! Y coordinate;
SCRATCH[3] ! Z coordinate;
)));
END "avia";
END
ELSE BEGIN ! A hand;
IF GETNUMS(SCRATCH,INPSTR,1)
THEN BEGIN "hvia" ! A via point has been requested;
VIAS ← TRUE;
MES ← MES & " |" & INPSTR;
PTR ← NEW_SVAL(SCRATCH[1]);
END "hvia";
END;
IF VIAS
THEN BEGIN "vias"
VIAPTR ← CELL:CAR[CEL] ← NEW_RECORD(VIA);
VIA:PLACE[VIAPTR] ← NEW_RECORD(VARIABLE);
VIA:ACTPLACE[VIAPTR] ← NEW_RECORD(DEXPR);
VARIABLE:VAL[VIA:PLACE[VIAPTR]] ← PTR;
DEXPR:VAL[VIA:ACTPLACE[VIAPTR]] ← PTR;
CEL ← CELL:CDR[CEL] ← NEW_RECORD(CELL);
IF FRILLS
THEN BEGIN "constr" ! Constraints for this VIA;
OUTSTR("Relation and time for via segment: ");
LODED("= 2 (clear this if you don't want it)" & CR);
STR ← INCHWL;
CHAR ← LOP(STR);
IF CHAR ≠ NULL
THEN BEGIN
RPTR(DURATION) DUR;
DUR ← VIA:TIME[VIAPTR] ← NEW_RECORD(DURATION);
IF CHAR=">" THEN DURATION:TIME_RELN[DUR] ← 1
ELSE IF CHAR="<" THEN DURATION:TIME_RELN[DUR] ← 2
ELSE IF CHAR="=" THEN DURATION:TIME_RELN[DUR] ← 3
ELSE OUTSTR("(No constraint)" & CRLF);
DURATION:TIME[DUR] ← NEW_RECORD(SVAL);
SVAL:VAL[DURATION:TIME[DUR]] ← REALSCAN(STR,BRCHAR);
END;
OUTSTR("Velocity for this via point, inches/second: ");
SCRA1[1] ← SCRA1[2] ← SCRA1[3] ← 0.0;
IF ARM ≤ 1
THEN BEGIN ! An arm;
IF GETNUMS(SCRA1,INPSTR,3)
THEN BEGIN "avel"
MES ← MES & " (" & INPSTR & ")";
VIA:VELOC[VIAPTR] ← NEW_V3ECT(
SCRA1[1], ! X velocity;
SCRA1[2], ! Y velocity;
SCRA1[3] ! Z velocity;
);
END "avel";
END
ELSE BEGIN ! A hand;
IF GETNUMS(SCRA1,INPSTR,1)
THEN BEGIN "hvel"
MES ← MES & " (" & INPSTR & ")";
VIA:VELOC[VIAPTR] ← NEW_SVAL(SCRA1[1]);
END "hvel";
END;
END "constr";
END "vias"
ELSE DONE "stuff";
END "stuff";
IF FRILLS AND (ARM ≤ 1)
THEN BEGIN "approach"
OUTSTR(CRLF & "Approach point; blank means none. ");
IF GETNUMS(SCRATCH,INPSTR,3)
THEN BEGIN ! An approach point has been requested;
MES ← MES & " |D:" & INPSTR;
PTR ← NEW_FRAME(NEW_TRANS(ROT,NEW_V3ECT(
SCRATCH[1], ! X coordinate;
SCRATCH[2], ! Y coordinate;
SCRATCH[3] ! Z coordinate;
)));
DEPPTR ← CELL:CAR[CEL] ← NEW_RECORD(DEPROACH);
DEPROACH:FRAME[DEPPTR] ← NEW_RECORD(VARIABLE);
VARIABLE:VAL[DEPROACH:FRAME[DEPPTR]] ← PTR;
DEPROACH:DEPARTURE[DEPPTR] ← FALSE;
CEL ← CELL:CDR[CEL] ← NEW_RECORD(CELL);
END;
END "approach";
! Final point;
OUTSTR(CRLF & "Final point, please. ");
IF ARM ≤ 1
THEN BEGIN ! An arm;
GETNUMS(SCRATCH,INPSTR,3);
MES ← MES & " |" & INPSTR;
PTR ← NEW_FRAME(NEW_TRANS(ROT,NEW_V3ECT
(
SCRATCH[1], ! X coordinate;
SCRATCH[2], ! Y coordinate;
SCRATCH[3] ! Z coordinate;
)));
END
ELSE BEGIN ! An arm;
GETNUMS(SCRATCH,INPSTR,1);
MES ← MES & " |" & INPSTR;
PTR ← NEW_SVAL(SCRATCH[1]);
END;
VARIABLE:VAL[MOVE$:DEST[MOV]] ← PTR;
DEXPR:VAL[MOVE$:DEXP[MOV]] ← PTR;
IF FRILLS
THEN BEGIN "globcnst" ! Global time constraint;
OUTSTR("Relation and time for whole motion: ");
LODED("= 3 (clear this if you don't want it)" & CR);
STR ← INCHWL;
CHAR ← LOP(STR);
TR ← 0;
IF CHAR = ">" THEN TR ← 1
ELSE IF CHAR = "<" THEN TR ← 2
ELSE IF CHAR = "=" THEN TR ← 3
ELSE OUTSTR("(No constraint this time.)" & CRLF);
IF TR THEN
BEGIN
PTR ← CELL:CAR[CEL] ← NEW_RECORD(DURATION);
DURATION:TIME_RELN[PTR] ← TR;
DURATION:TIME[PTR] ← NEW_RECORD(SVAL);
SVAL:VAL[DURATION:TIME[PTR]] ← REALSCAN(STR,BRCHAR);
END
END "globcnst";
TRJCLC(MOV,WORLD);
CLOSEOUT;
END "maketfile";
END $$prgid;